//
// Created by Pulsar on 2021/8/26.
//

#include <libapriltags/libapriltags.h>
#include <rc_system/functions.h>
#include <vector>
#include <libapriltags/apriltag.h>
#include <libapriltags/apriltag_pose.h>
#include <libapriltags/tag16h5.h>
#include <libapriltags/tag25h9.h>
#include <libapriltags/tag36h11.h>
#include <libapriltags/tagCircle21h7.h>
#include <libapriltags/tagCircle49h12.h>
#include <libapriltags/tagCustom48h12.h>
#include <libapriltags/tagStandard41h12.h>
#include <libapriltags/tagStandard52h13.h>
#include <utility>
#include <rc_log/slog.hpp>

void *InitCallback() {

}

void *BeforeDetcetCallback(void *args) {

}

void DetectCallback(cv::Mat &frame, void *args, const std::shared_ptr<rccore::message::MoveMessage> &p_move_message) {
    apriltag_family_t *tf = tag36h11_create();
    apriltag_detector_t *td = apriltag_detector_create();
    apriltag_detector_add_family(td, tf);
    td->quad_decimate = 1.0;

    WHEEL_DATA wheelData = {0.0, 0.0, 0.0, 0.0};
    p_move_message->push_message(wheelData);
    cv::Mat gray;
    cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
    image_u8_t im = {
            .width = gray.cols, .height = gray.rows, .stride = gray.cols, .buf = gray.data};

    zarray_t *detections = apriltag_detector_detect(td, &im);

    size_t tag_number = zarray_size(detections);
    apriltag_detection_t min_det;

    for (size_t i = 0; i < tag_number; i++) {
        apriltag_detection_t *det;
        zarray_get(detections, i, &det);
        line(
                frame, cv::Point(det->c[0], det->c[1]), cv::Point(frame.cols / 2, frame.rows / 2),
                cv::Scalar(255, 255, 0), 3);

        line(
                frame, cv::Point(det->p[0][0], det->p[0][1]), cv::Point(det->p[1][0], det->p[1][1]),
                cv::Scalar(0, 0, 0xff), 2);
        line(
                frame, cv::Point(det->p[0][0], det->p[0][1]), cv::Point(det->p[3][0], det->p[3][1]),
                cv::Scalar(0, 0xff, 0), 2);
        line(
                frame, cv::Point(det->p[1][0], det->p[1][1]), cv::Point(det->p[2][0], det->p[2][1]),
                cv::Scalar(0xff, 0, 0), 2);
        line(
                frame, cv::Point(det->p[2][0], det->p[2][1]), cv::Point(det->p[3][0], det->p[3][1]),
                cv::Scalar(0xff, 0, 0), 2);

        std::stringstream tag_id;
        tag_id << det->id;
        cv::String text = tag_id.str();
        int baseline;

        cv::Size text_size = cv::getTextSize(text, cv::FONT_HERSHEY_SIMPLEX, 1.0, 2, &baseline);

        putText(
                frame, text,
                cv::Point(det->c[0] - text_size.width / 2, det->c[1] + text_size.height / 2),
                cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0xff, 0x99, 0), 2);

    }

    slog::info << frame.size() << slog::endl;
    cv::imshow("frame", frame);
    cv::waitKey(1);
    apriltag_detector_destroy(td);
}

void *AfterDetcetCallback(void *args) {

}

void OnThreadBreakCallback(void *args) {

}